
/***************************************************************************** \
 *                              ColWrapperBullet
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *    Wrapper functions, which convert the Bullet rigid body into the
 *    corresponding CollDet objects.
 *
 *  Normally these wrapper functions will be used as callback
 *  function, pass to the collision pipeline as a parameter
 *
 *  see makeCollidable
 *
 *  @author Weiyu Yi, wyi@tu-clausthal.de
 *
 */


//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdio.h>

#define COL_EXPORT

#include <col_import_export.h>
#include <ColWrapperBullet.h>

/// Collision detection namespace
namespace col {

/** callback function which convert the Bullet shape into corresponding
 * colldet geometry.
 *
 * @param ColGeometry*  pointer to a tobe assigned colldet geometry @param
 * const void*   pointer to the Bullet btGImpackMeshShape object
 *
 * the Bullet shape's pointer has to be changed into void* type for the
 * consistence of the pipeline interface.
 *
 */
bool convertGeometryFromBulletShape( ColGeometry *geo, void *geo_source )
{
	// assert
	
	// clear the old data in geometry
	geo->clearOldData();

    // cast the pointer and preprocessing
	btGImpactMeshShape *source = static_cast<btGImpactMeshShape*> (geo_source);

    unsigned int vertexCountSinceLastPart = 0;
    for ( unsigned int i=0; i < source->getMeshPartCount(); i++ )
    {
        btGImpactMeshShapePart *meshPart = source->getMeshPart( i );
        const btPrimitiveManagerBase *primMgr  = source->getPrimitiveManager();
        const btGImpactMeshShapePart::TrimeshPrimitiveManager *triMgr  = dynamic_cast<const btGImpactMeshShapePart::TrimeshPrimitiveManager*> (primMgr);
        if ( ! triMgr->is_trimesh() )
        {
            printf( "only triangle mesh is supported now by CollDet\n" );
            return false;
        }

        // copy the points
        int vertexCount = triMgr->get_vertex_count();
        geo->editPointsPtr()->resize( vertexCount );
        for ( unsigned int i=0; i < vertexCount; i++ )
        {
            btVector3 vec;
            triMgr->get_vertex( i, vec );
            (*(geo->editPointsPtr()))[i][0] = vec[0];
            (*(geo->editPointsPtr()))[i][1] = vec[1];
            (*(geo->editPointsPtr()))[i][2] = vec[2];
        }

        // copy the primitives
        int primCount = triMgr->get_primitive_count();
        geo->editPrimitivesPtr()->resize( primCount );
        for ( unsigned int i=0; i < primCount; i++ )
        {
            int i0, i1, i2;
            triMgr->get_indices( i, i0, i1, i2 );

            (*geo->editPrimitivesPtr())[i].push_back( i0 + vertexCountSinceLastPart );
            (*geo->editPrimitivesPtr())[i].push_back( i1 + vertexCountSinceLastPart );
            (*geo->editPrimitivesPtr())[i].push_back( i2 + vertexCountSinceLastPart );
        }

        // bind the separate mesh part together
        vertexCountSinceLastPart += vertexCount;
    }
	
	// update bbox
	geo->updateBBox();

	return true;
}

/** callback function which convert the Bullet transform into colldet
 * transformation matrix.
 *
 * @param Matrix4*  pointer to the colldet transformation matrix @param const
 * void*  pointer to the Bullet transform
 *
 * see convertGeometryFromBullet
 */
bool convertMatrixFromBulletTransform( Matrix4 *tran, void *tran_source )
{
	const btTransform *btTran = static_cast<const btTransform*>(tran_source);

	// tran has be allocated but not be initialized
	// so all vertors in matrix must be assigned.
    btMatrix3x3 basis = btTran->getBasis();
    btVector3 origin = btTran->getOrigin();

    Vector4 vec0( basis.getColumn(0)[0], basis.getColumn(0)[1], basis.getColumn(0)[2], 0.0 );
    Vector4 vec1( basis.getColumn(1)[0], basis.getColumn(1)[1], basis.getColumn(1)[2], 0.0 );
    Vector4 vec2( basis.getColumn(2)[0], basis.getColumn(2)[1], basis.getColumn(2)[2], 0.0 );
    Vector4 vec3( origin[0], origin[1], origin[2], origin[3] );
    //Vector3 vec1( getCol1[0], btTran->getCol1[1], btTran->getCol1[2], 0.0 );
    //Vector3 vec2( getCol2[0], btTran->getCol2[1], btTran->getCol2[2], 0.0 );
    //Vector3 vec3( btTran->getCol3[0], btTran->getCol3[1], btTran->getCol3[2], 1.0 );

    tran->setCol0( vec0 );
    tran->setCol1( vec1 );
    tran->setCol2( vec2 );
    tran->setCol3( vec3 );

	return true;
}

} //namespace col
